/*
 * Vector3D.c
 *
 * Created: 13-Jan-13 1:14:49 AM
 *  Author: M.Hefny
 * Code is adapted from http://code.google.com/p/picquadcontroller/
 * please access his DCM article http://www.starlino.com/dcm_tutorial.html
 */ 


#include "../Include/Vector3D.h"

//get modulus of a 3d vector sqrt(x^2+y^2+y^2)
float vector3d_modulus(float* vector)
{
        static float R;  
        R = vector[0]*vector[0];
        R += vector[1]*vector[1];
        R += vector[2]*vector[2];
        return sqrt(R);
}

//convert vector to a vector with same direction and modulus 1
void vector3d_normalize(float* vector)
{
        static float R;  
        R = vector3d_modulus(vector);
        vector[0] /= R;
        vector[1] /= R; 
        vector[2] /= R;  
}

//calcuate vector dot-product  c = a . b
float vector3d_dot(float* a,float* b)
{
        return a[0]*b[0]+a[1]*b[1]+a[2]*b[2];
}


//calcuate vector cross-product  c = a x b
void vector3d_cross(float* a,float* b, float* c)
{
        c[0] = a[1]*b[2] - a[2]*b[1];
        c[1] = a[2]*b[0] - a[0]*b[2];
        c[2] = a[0]*b[1] - a[1]*b[0];
}

//calcuate vector scalar-product  n = s x a
void vector3d_scale(float s, float* a , float* b)
{
        b[0] = s*a[0];
        b[1] = s*a[1];
        b[2] = s*a[2];
} 


//calcuate vector sum   c = a + b
void vector3d_add(float* a , float* b, float* c)
{
        c[0] = a[0] + b[0];
        c[1] = a[1] + b[1];
        c[2] = a[2] + b[2];
} 


//creates equivalent skew symetric matrix plus identity
//for v = {x,y,z} returns
// m = {{1,-z,y}
//              {z,1,-x}
//              {-y,x,1}}
void vector3d_skew_plus_identity(float *v,float* m)
{
        m[0*3+0]=1;
        m[0*3+1]=-v[2];
        m[0*3+2]=v[1];
        m[1*3+0]=v[2];
        m[1*3+1]=1;
        m[1*3+2]=-v[0];
        m[2*3+0]=-v[1];
        m[2*3+1]=v[0];
        m[2*3+2]=1;
}
